#include <Firmata.h>

#include <FSKModem.h>

FSKModem modem(0, 13);

int MORTOR0_PIN  = 9;
int MORTOR1_PIN  = 10;
int _status = 0;
int _pinum  = 0;
unsigned long _time = 0;

// The setup() method runs once, when the sketch starts
void setup()   {             
  modem.begin();

  Serial.begin(9600);
  Serial.print("Begin (rev07) : \n");
}

// the loop() method runs over and over again,
// as long as the Arduino has power
void loop()                     
{  
  int data = modem.read();
  if(data != -1) 
  {
  switch(_status) { 
    case 0:  // header
      if(data == 0xE0 ) {
        _status = 1;
      }
    break;
    case 1:  // pin number
      _pinum = data;
      if(_pinum == MORTOR0_PIN || _pinum == MORTOR1_PIN) { 
        _status = 2;
      } else {
        _status = 0;
      }
    break;
    case 2:  // power      
      analogWrite(_pinum, (byte)(data << 1));
      _time = millis();
      _status = 0;
    break;
    default:  _status = 0; break;
  }
  }
  // safety stop
  unsigned long last_updated = millis() - _time;
  if(last_updated > 500) {
    analogWrite(MORTOR0_PIN, 0);
    analogWrite(MORTOR1_PIN, 0);
  }  
}
